#include "PID.h"
#include "led.h"
#include "PWM.h"
#include "sys.h"




//Rol方向pid
float PID_Rol_PosCalc(float NextPoint)
{
  register float  iError,dError;
	iError = Rol_Set_Pos - NextPoint;        // 偏差
	Rol_SumError += iError;				    				// 积分
	if(Rol_SumError > 950.0)									//积分限幅1000
		Rol_SumError = 950.0;
	else if(Rol_SumError < -950.0)
		Rol_SumError = -950.0;	
	dError = iError - Rol_LastError; 					// 当前微分
	//dError = (iError - Rol_LastError) * filter_factor + dError * (1 - filter_factor);	// 滤波建议
	Rol_LastError = iError;
	
	return(rol_p * iError + rol_i * Rol_SumError + rol_d * dError);//返回计算值
}

//Pit方向上PID算法
float PID_Pit_PosCalc(float NextPoint)
{
  register float  iError,dError;
	iError = Pit_Set_Pos - NextPoint;        // 偏差
	Pit_SumError += iError;				    				// 积分
	if(Pit_SumError > 950.0)									//积分限幅1000
		Pit_SumError = 950.0;
	else if(Pit_SumError < -950.0)
		Pit_SumError = -950.0;	
	dError = iError - Pit_LastError; 					// 当前微分
	Pit_LastError = iError;
	
	return(pit_p * iError + pit_i * Pit_SumError + pit_d * dError);//返回计算值
}



//void Pit_Mot_Con(short linshi)
//{
//	if(linshi<=-1000)
//			{
//				TIM3->CCR4 = 1000;
//				TIM3->CCR3 = 1000;
//			}
//			else if(linshi <= 0)
//			{
//				TIM3->CCR4 = 1000+linshi;
//				TIM3->CCR3 = 1000;
//			}
//			else if(linshi<1000)
//			{
//				TIM3->CCR4 = 1000;
//				TIM3->CCR3 = 1000-linshi;
//			}
//			else
//			{
//				TIM3->CCR4 = 1000;
//				TIM3->CCR3 = 1000;
//			}
//}
void Pit_Mot_Con(short linshi)
{
	if(linshi<=-1000)
			{
				TIM3->CCR3 = 1000;
				TIM3->CCR4 = 1000;
			}
			else if(linshi <= 0)
			{
				TIM3->CCR3 = 1000+linshi;
				TIM3->CCR4 = 1000;
			}
			else if(linshi<1000)
			{
				TIM3->CCR3 = 1000;
				TIM3->CCR4 = 1000-linshi;
			}
			else
			{
				TIM3->CCR3 = 1000;
				TIM3->CCR4 = 1000;
			}
}
//PIt方向上电机控制
void Rol_Mot_Con(short linshi)
{
	if(linshi<=-1000)
			{
				TIM3->CCR1 = 1000;
				TIM3->CCR2 = 1000;
			}
			else if(linshi <= 0)
			{
				TIM3->CCR1 = 1000+linshi;
				TIM3->CCR2 = 1000;
			}
			else if(linshi<1000)
			{
				TIM3->CCR1 = 1000;
				TIM3->CCR2 = 1000-linshi;
			}
			else
			{
				TIM3->CCR1 = 1000;
				TIM3->CCR2 = 1000;
			}
}

